/*
  SoftwareSerial2.cpp - Software serial library
  Copyright (c) 2006 David A. Mellis.  All right reserved.
  Modified by ladyada and tats

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 3 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

/******************************************************************************
 * Includes
 ******************************************************************************/
#include <avr/interrupt.h>
#include <stdlib.h>
#include <string.h>
#include "WConstants.h"
#include "SoftwareSerial2.h"

/******************************************************************************
 * Statics
 ******************************************************************************/
// This function works right at 16MHz. This is why the _bitDelay needs adjustment (see below).
void whackDelay(uint16_t delay) { 
  uint8_t tmp=0;

  asm volatile("sbiw    %0, 0x01 \n\t" // subtracts 1 from delay
	       "ldi %1, 0xFF \n\t"           // loads -1 to temp
	       "cpi %A0, 0xFF \n\t"          // compare low order byte of delay to -1
	       "cpc %B0, %1 \n\t"            // compare high order byte of delay to temp, (with carry)
	       "brne .-10 \n\t"              // repeat if not equal
	       : "+r" (delay), "+a" (tmp)    // this tells us the first param is delay and the second is temp
	       : "0" (delay)                 // this tells us that delay will not survive
	       );
}

bool SoftwareSerial2::_initialized = 0;
char *SoftwareSerial2::_receive_buffers[14];
uint8_t SoftwareSerial2::_receive_buffers_index[14];
int SoftwareSerial2::_bitDelay = 0;
long SoftwareSerial2::_baudRate = 0;


void SoftwareSerial2::receive() {
  bool pinReady[14];
  unsigned char pinStep[14], data[14];
  unsigned char nPinReady = 0, nReceiveBuffers = 0;
  char pin;
  volatile uint16_t waitTime;
  
  // Init.
  for (pin=0; pin<=13; pin++) {
    if (_receive_buffers[pin]) {
      nReceiveBuffers++; // XXX should be static
      if (!digitalRead(pin)) {
        pinReady[pin] = true;
        pinStep[pin] = 0;
        nPinReady++;
        data[pin] = 0;
      } else {
        pinReady[pin] = false;
      }
    }
  }

  waitTime = _bitDelay - 6*nReceiveBuffers - 2*nPinReady;
  
  // Loop.
  while (nPinReady > 0) {
    // Wait for byte.
    whackDelay(waitTime);
    waitTime = _bitDelay*2 - 6*nReceiveBuffers - 6*nPinReady;
    
    for (pin=0; pin<=13; pin++) {
      if (_receive_buffers[pin]) {
        
        if (!pinReady[pin]) {
          // Check if the pin received a startbit.
          if (!digitalRead(pin)) {
            pinReady[pin] = true;
            pinStep[pin] = 0;
            nPinReady++;
          }
        } else {
          // Pin is ready, read next bit.
          if (digitalRead(pin))
            data[pin] |= (1 << pinStep[pin]);
          pinStep[pin]++;
  
          // Check if byte has been read on that pin.
          if (pinStep[pin] >= 8) {
            pinReady[pin] = false;
            if (SoftwareSerial2::_receive_buffers_index[pin] < AFSS_MAX_RX_BUFF) {
              SoftwareSerial2::_receive_buffers[pin][SoftwareSerial2::_receive_buffers_index[pin]] = data[pin]; // save data 
              SoftwareSerial2::_receive_buffers_index[pin]++;  // got a byte 
            }
            data[pin] = 0;
            nPinReady--;
          }
        }
      }
    }
  }
}

//
//void SoftwareSerial2::recv() { 
//  char i;
//  bool pinReady[14];
//  char d[14];
//  char nReceiveBuffers = 0, nPinReady = 0;
//  char pin;
//  
//  // Init (read startbit).
//  for (pin=0; pin<=13; pin++) {
//    if (_receive_buffers[pin]) {
//      nReceiveBuffers++;
//      pinReady[pin] = !digitalRead(pin);     // is it ready?
//      if (pinReady[pin]) {
//        d[pin] = 0;
//        nPinReady++;
//      }
//    }
//  }
//  if (nPinReady > 0) {
//    // Wait for byte.
//    whackDelay(_bitDelay - 6*nReceiveBuffers - 2*nPinReady); // digitalread and operations takes some time
//  
//    // Incoming byte.
//    for (i=0; i<8; i++) { 
//      whackDelay(_bitDelay*2 - 6*nPinReady);  // digitalread takes some time
//      for (pin=0; pin<=13; pin++) {
//        if (pinReady[pin] && digitalRead(pin)) 
//          d[pin] |= (1 << i);
//      }
//    }
//    whackDelay(_bitDelay*2);
//    for (pin=0; pin<=13; pin++) {
//      if (pinReady[pin] && SoftwareSerial2::_receive_buffers_index[pin] < AFSS_MAX_RX_BUFF) {
//        SoftwareSerial2::_receive_buffers[pin][SoftwareSerial2::_receive_buffers_index[pin]] = d[pin]; // save data 
//        SoftwareSerial2::_receive_buffers_index[pin]++;  // got a byte 
//      }
//    }
//  }
//} 

/******************************************************************************
 * Interrupts
 ******************************************************************************/

// NEEDS FIX This code is flawed. It will not work if data is received at the same time on different pins.
SIGNAL(SIG_PIN_CHANGE0) {
  SoftwareSerial2::receive();
}
SIGNAL(SIG_PIN_CHANGE2)
{
  SoftwareSerial2::receive();
}  

/******************************************************************************
 * Constructors
 ******************************************************************************/

SoftwareSerial2::SoftwareSerial2(uint8_t receivePin, uint8_t transmitPin)
{
  if (!_initialized) {
    for (int i=0; i<=13; i++)
      _receive_buffers[i] = 0; // Initialize to NULL.
    _initialized = true; 
  }
  setRX(receivePin);
  setTX(transmitPin);
}

void SoftwareSerial2::setTX(uint8_t tx) {
  _transmitPin = tx;
  pinMode(_transmitPin, OUTPUT);
  digitalWrite(_transmitPin, HIGH);
}

void SoftwareSerial2::setRX(uint8_t rx) {
  _receivePin = rx;

  // Note: this will crash any previous _receiveBuffer at the same _receivePin
  _receive_buffers[_receivePin] = _receiveBuffer;
  _receive_buffers_index[_receivePin] = 0;

  pinMode(_receivePin, INPUT); 
  digitalWrite(_receivePin, HIGH);  // pullup!

  if (_receivePin < 8) {
    // a PIND pin, PCINT16-23
    PCMSK2 |= _BV(_receivePin);
    PCICR |= _BV(2);
  } else if (_receivePin <= 13) {
    // a PINB pin, PCINT0-5
    PCMSK0 |= _BV(_receivePin-8);
    PCICR |= _BV(0);    
  } 
}

/******************************************************************************
 * User API
 ******************************************************************************/

void SoftwareSerial2::begin(long speed)
{
  _baudRate = speed;
  switch (_baudRate) {
  case 115200: // For xmit -only-!
    _bitDelay = 4; break;
  case 57600:
    _bitDelay = 14; break;
  case 38400:
    _bitDelay = 24; break;
  case 31250:
    _bitDelay = 31; break;
  case 19200:
    _bitDelay = 54; break;
  case 9600:
    _bitDelay = 113; break;
  case 4800:
    _bitDelay = 232; break;
  case 2400:
    _bitDelay = 470; break;
  default:
    _bitDelay = 0;
  }
  // Adjust the _bitDelay.
  _bitDelay = ((unsigned long)_bitDelay * F_CPU) / 16000000L;

  whackDelay(_bitDelay*2); // if we were low this establishes the end
}

int SoftwareSerial2::read(void)
{
  uint8_t d,i;

  if (! _receive_buffers_index[_receivePin])
    return -1;

  d = _receiveBuffer[0]; // grab first byte
  // if we were awesome we would do some nifty queue action
  // sadly, i dont care
  for (i=0; i<_receive_buffers_index[_receivePin]; i++) {
    _receiveBuffer[i] = _receiveBuffer[i+1];
  }
  _receive_buffers_index[_receivePin]--;
  return d;
}

uint8_t SoftwareSerial2::available(void)
{
  return _receive_buffers_index[_receivePin];
}

void SoftwareSerial2::print(uint8_t b)
{
  if (_baudRate == 0)
    return;
  byte mask;

  cli();  // turn off interrupts for a clean txmit

  digitalWrite(_transmitPin, LOW);  // startbit
  whackDelay(_bitDelay*2);

  for (mask = 0x01; mask; mask <<= 1) {
    if (b & mask){ // choose bit
      digitalWrite(_transmitPin,HIGH); // send 1
    }
    else{
      digitalWrite(_transmitPin,LOW); // send 1
    }
    whackDelay(_bitDelay*2);
  }
  
  digitalWrite(_transmitPin, HIGH);
  sei();  // turn interrupts back on. hooray!
  whackDelay(_bitDelay*2);
}

void SoftwareSerial2::print(const char *s)
{
  while (*s)
    print(*s++);
}

void SoftwareSerial2::print(char c)
{
  print((uint8_t) c);
}

void SoftwareSerial2::print(int n)
{
  print((long) n);
}

void SoftwareSerial2::print(unsigned int n)
{
  print((unsigned long) n);
}

void SoftwareSerial2::print(long n)
{
  if (n < 0) {
    print('-');
    n = -n;
  }
  printNumber(n, 10);
}

void SoftwareSerial2::print(unsigned long n)
{
  printNumber(n, 10);
}

void SoftwareSerial2::print(long n, int base)
{
  if (base == 0)
    print((char) n);
  else if (base == 10)
    print(n);
  else
    printNumber(n, base);
}

void SoftwareSerial2::println(void)
{
  print('\r');
  print('\n');  
}

void SoftwareSerial2::println(char c)
{
  print(c);
  println();  
}

void SoftwareSerial2::println(const char c[])
{
  print(c);
  println();
}

void SoftwareSerial2::println(uint8_t b)
{
  print(b);
  println();
}

void SoftwareSerial2::println(int n)
{
  print(n);
  println();
}

void SoftwareSerial2::println(long n)
{
  print(n);
  println();  
}

void SoftwareSerial2::println(unsigned long n)
{
  print(n);
  println();  
}

void SoftwareSerial2::println(long n, int base)
{
  print(n, base);
  println();
}

// Private Methods /////////////////////////////////////////////////////////////

void SoftwareSerial2::printNumber(unsigned long n, uint8_t base)
{
  unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. 
  unsigned long i = 0;

  if (n == 0) {
    print('0');
    return;
  } 

  while (n > 0) {
    buf[i++] = n % base;
    n /= base;
  }

  for (; i > 0; i--)
    print((char) (buf[i - 1] < 10 ? '0' + buf[i - 1] : 'A' + buf[i - 1] - 10));
}

